/*
 * manual_control.h
 *
 *  Created on: Jan 24, 2013
 *      Author: michiel
 */

#ifndef MANUAL_CONTROL_H_
#define MANUAL_CONTROL_H_

#include <sensor_msgs/Joy.h>
#include <std_msgs/Float64.h>

#include "servers/base_server.h"
#include "PS3Controller/PS3_buttons.h"
#include "rp_states/ManualControlAction.h"

class ManualControl : public BaseServer {
private:
	bool _trunk_up_button;
	bool _trunk_down_button;
	bool _trunk_neutral_button;

	bool _gripper_open_button;
	bool _gripper_closed_button;

	bool _server_stop;

	float _eyes_pitch;
	float _eyes_yaw;

	ros::Time	_end_time;

	actionlib::SimpleActionServer<rp_states::ManualControlAction> _as;
protected:
	ros::Subscriber controller_sub_;
	ros::Publisher eyes_pitch_pub_;
	ros::Publisher eyes_left_pub_;
	ros::Publisher eyes_right_pub_;

	rp_states::ManualControlFeedback	feedback_;
	rp_states::ManualControlResult		result_;
	void controllerCallback(const sensor_msgs::Joy::ConstPtr &msg);
public:
	ManualControl(std::string name);
	~ManualControl();
	void execute(const rp_states::ManualControlGoalConstPtr &goal);
};


#endif /* MANUAL_CONTROL_H_ */
